#ifndef CELL_HPP_
#define CELL_HPP_

#include <Eigen/Core>
#include <stdexcept>
#include <iostream>

/* dim=3, fe=(order+1)(order+2)(order+3)/6
  dim=2, fe=(order+1)(order+2)/2 */
template <int order = 1> class TetCell {
private:
  Eigen::Matrix3d M;   // 坐标变换矩阵
  Eigen::Vector3d x_3; // x = M*lambda + x_3
  Eigen::Matrix3d M_inv;
  double J_M; // M的行列式
  int fe = (order + 1) * (order + 2) * (order + 3) / 6;

public:
  TetCell(Eigen::Vector3d p0, Eigen::Vector3d p1, Eigen::Vector3d p2,
          Eigen::Vector3d p3) {
    x_3 = p3;
    M.setZero();
    /*M = [x0-x3,x1-x3,x2-x3]
          [y0-y3,y1-y3,y2-y3]
          [z0-z3,z1-z3,z2-z3]
      x = M*lambda + x3*/
    // for (int i = 0; i < 3; i++) {
    //   M.col(i) = -coordinate.row(3);
    // }
    M.col(0) = p0 - p3;
    M.col(1) = p1 - p3;
    M.col(2) = p2 - p3;
    M_inv = M.inverse();
    J_M = std::abs(M.determinant());
  }

  ~TetCell() {}

  double value(int index, const Eigen::Vector3d &point) const {
    if (order == 1) {
      if (index >= fe) {
        throw std::invalid_argument("index not in range");
      } else if (index == 3) {
        return 1 - point(0) - point(1) - point(2);
      } else {
        return point(index);
      }
    } else {
      /*未实现*/
      return 0;
    }
  }

  Eigen::Vector3d gradient(int index, const Eigen::Vector3d &point) const {
    Eigen::Vector3d grad;
    grad.setZero();
    for (int i = 0; i < 3; i++) {
      Eigen::Vector<double, 3> shift;
      shift.setZero();
      double delta = 1e-5;
      shift(i) = delta;
      grad(i) = (value(index, point + shift) - value(index, point)) / delta;
    }
    return grad.transpose() * M_inv;
  }

  double jacobi() const { return J_M; }

  Eigen::Vector3d map(Eigen::Vector3d &x) {
    /* lambda = M_inv*(x - x3) */
    /* x = M*lambda + x3*/
    return M * x + x_3;
  }

  int fe_per_cell() { return fe; }
};


// 六面体单元
// 暂且考虑长方体单元
class HexCell {
private:
  Eigen::Matrix3d M; // 坐标变换矩阵（从标准单元到目标单元）
  Eigen::Matrix3d M_inv; // 坐标变换矩阵（从目标单元到标准单元）
  Eigen::Vector3d x_shift; // 坐标变换的平移向量（从标准单元到目标单元）
  double J_M;              // M的行列式
  int fe = 8;

public:
  HexCell(const Eigen::Vector3d &start, const Eigen::Vector3d &end) {
    M.setZero();
    M_inv.setZero();
    x_shift.setZero();
    for (int i = 0; i < 3; i++) {
      M(i, i) = (end(i) - start(i)) / 2.0;
      M_inv(i, i) = 2.0 / (end(i) - start(i));
      x_shift(i) = (end(i) + start(i)) / 2.0;
    }
    J_M = std::abs(M.determinant());

    // std::cout << "M" << std::endl;
    // std::cout << M << std::endl;
  }

  ~HexCell() {}

  double value(int index, const Eigen::Vector3d &x) const {
    switch (index) {
    case 0:
      return (1 - x[0]) * (1 - x[1]) * (1 - x[2]) / 8.0;
    case 1:
      return (1 + x[0]) * (1 - x[1]) * (1 - x[2]) / 8.0;
    case 2:
      return (1 + x[0]) * (1 + x[1]) * (1 - x[2]) / 8.0;
    case 3:
      return (1 - x[0]) * (1 + x[1]) * (1 - x[2]) / 8.0;
    case 4:
      return (1 - x[0]) * (1 - x[1]) * (1 + x[2]) / 8.0;
    case 5:
      return (1 + x[0]) * (1 - x[1]) * (1 + x[2]) / 8.0;
    case 6:
      return (1 + x[0]) * (1 + x[1]) * (1 + x[2]) / 8.0;
    case 7:
      return (1 - x[0]) * (1 + x[1]) * (1 + x[2]) / 8.0;
    default:
      throw std::invalid_argument("invalid index");
    }
  }

  Eigen::Vector3d gradient(int index, const Eigen::Vector3d &point) const {
    Eigen::Vector3d grad;
    grad.setZero();
    for (int i = 0; i < 3; i++) {
      Eigen::Vector<double, 3> shift;
      shift.setZero();
      double delta = 1e-5;
      shift(i) = delta;
      grad(i) = (value(index, point + shift) - value(index, point)) / delta;
    }
    return grad.transpose() * M_inv;
  }

  double jacobi() const { return J_M; }

  Eigen::Vector3d map(Eigen::Vector3d &x) { return M * x + x_shift; }

  int fe_per_cell() { return fe; }
};

#endif